#pragma once
#include <math.h>

class Float3
{
  public:
    Float3() : X(0.0f), Y(0.0f), Z(0.0f) { }
    Float3(float x, float y, float z) : X(x), Y(y), Z(z) { }

    float X;
    float Y;
    float Z;

    Float3 normalize() const
    {
      float norm = sqrtf(X * X + Y * Y + Z * Z);

      return Float3(X / norm, Y / norm, Z / norm);
    }

    float norm() const
    {
      return sqrtf(X * X + Y * Y + Z * Z);
    }

    Float3 cos() const
    {
      return Float3(::cos(X), ::cos(Y), ::cos(Z));
    }

    Float3 sin() const
    {
      return Float3(::sin(X), ::sin(Y), ::sin(Z));
    }

    Float3 operator-(const Float3& b) const
    {
      Float3 val(this->X - b.X, this->Y - b.Y, this->Z - b.Z);

      return val;
    }

    Float3 operator+(const Float3& b) const
    {
      Float3 val(this->X + b.X, this->Y + b.Y, this->Z + b.Z);

      return val;
    }

    Float3 operator*(const float& b) const
    {
      return Float3(X * b, Y * b, Z * b);
    }

    Float3 operator-() const
    {
      return Float3(-X, -Y, -Z);
    }

    operator float*()
    {
      return &X;
    }

    void operator+=(const Float3& b)
    {
      X += b.X;
      Y += b.Y;
      Z += b.Z;
    }

    static float Dot(const Float3& a, const Float3& b)
    {
      return a.X * b.X + a.Y * b.Y + a.Z * b.Z;
    }

    static Float3 Cross(const Float3& a, const Float3& b)
    {
      return Float3(a.Y * b.Z - a.Z * b.Y,
                    a.Z * b.X - a.X * b.Z,
                    a.X * b.Y - a.Y * b.X);
    }
};

class AffineMatrix
{
  public:
    Float3 operator*(const Float3& x) const
    {
      Float3 y(0.0f, 0.0f, 0.0f);

      y.X = mData[0] * x.X + mData[4] * x.Y + mData[8] * x.Z + mData[12];
      y.Y = mData[1] * x.X + mData[5] * x.Y + mData[9] * x.Z + mData[13];
      y.Z = mData[2] * x.X + mData[6] * x.Y + mData[10] * x.Z + mData[14];

      return y;
    }

    AffineMatrix operator*(const AffineMatrix& A) const
    {
      AffineMatrix result;

      //Matrix multiplication
      for(unsigned int i = 0; i < 4; i++)
      {
        for(unsigned int j = 0; j < 4; j++)
        {
          //Unroll the k loop
          result.mData[i + j * 4] = mData[i + 0 * 4] * mData[j * 4 + 0];
          result.mData[i + j * 4] += mData[i + 1 * 4] * mData[j * 4 + 1];
          result.mData[i + j * 4] += mData[i + 2 * 4] * mData[j * 4 + 2];
          result.mData[i + j * 4] += mData[i + 3 * 4] * mData[j * 4 + 3];
        }
      }

      return result;
    }

  protected:
    float mData[4 * 4];
};

class RotationMatrix : public AffineMatrix
{
  public:
    RotationMatrix(Float3 angles)
    {
      Float3 sinA = angles.sin();
      Float3 cosA = angles.cos();

      //Fill column 1
      mData[0] = cosA.Y * cosA.Z;
      mData[1] = cosA.Y * sinA.Z;
      mData[2] = -sinA.Y;
      mData[3] = 0.0f;

      //Fill column 2
      mData[4] = -cosA.X * sinA.Z + sinA.X * sinA.Y * cosA.Z;
      mData[5] = cosA.X * cosA.Z + sinA.X * sinA.Y * sinA.Z;
      mData[6] = sinA.X * cosA.Y;
      mData[7] = 0.0f;

      //Fill column 3
      mData[8] = sinA.X * sinA.Z + cosA.X * sinA.Y * cosA.Z;
      mData[9] = -sinA.X * cosA.Z + cosA.X * sinA.Y * sinA.Z;
      mData[10] = cosA.X * cosA.Y;
      mData[11] = 0.0f;

      //Fill column 4
      mData[12] = 0.0f;
      mData[13] = 0.0f;
      mData[14] = 0.0f;
      mData[15] = 1.0f;
    }
};

class TranslationMatrix : public AffineMatrix
{
  public:
    TranslationMatrix(Float3 moves)
    {
      //Column 1
      mData[0] = 1.0f;
      mData[1] = 0.0f;
      mData[2] = 0.0f;
      mData[3] = 0.0f;

      //Column 2
      mData[4] = 0.0f;
      mData[5] = 1.0f;
      mData[6] = 0.0f;
      mData[7] = 0.0f;
      
      //Column 3
      mData[8] = 0.0f;
      mData[9] = 0.0f;
      mData[10] = 1.0f;
      mData[11] = 0.0f;

      //Column 4
      mData[4] = moves.X;
      mData[5] = moves.Y;
      mData[6] = moves.Z;
      mData[7] = 1.0f;
    }
};

class Ray
{
  public:
    Ray() { };
    Ray(const Float3& origin, const Float3& direction) : 
      Origin(origin), Direction(direction) { }

    Float3 Origin;
    Float3 Direction;
};

class RGBColor
{
  public:
    RGBColor() : Red(0.0f), Green(0.0f), Blue(0.0f) { }
    RGBColor(float red, float green, float blue) : 
      Red(red), Green(green), Blue(blue) 
    { 
    }

    void operator+=(const RGBColor& a)
    {
      Red += a.Red;
      Green += a.Green;
      Blue += a.Blue;
    }

    void operator*=(const RGBColor& a)
    {
      Red *= a.Red;
      Green *= a.Green;
      Blue *= a.Blue;
    }

    void operator*=(const Float3& a)
    {
      Red *= a.X;
      Green *= a.Y;
      Blue *= a.Z;
    }

    void operator*=(const float& a)
    {
      Red *= a;
      Green *= a;
      Blue *= a;
    }

    void operator+=(const Float3& a)
    {
      Red += a.X;
      Green += a.Y;
      Blue += a.Z;
    }

    RGBColor operator*(const float a)
    {
      return RGBColor(Red * a, Green * a, Blue * a);
    }

    RGBColor operator*(const RGBColor a)
    {
      return RGBColor(Red * a.Red, Green * a.Green, Blue * a.Blue);
    }

    RGBColor operator+(const RGBColor a)
    {
      return RGBColor(Red + a.Red, Green + a.Green, Blue + a.Blue);
    }

    float Red;
    float Green;
    float Blue;
};
